from painter import lidar_painter
from rplidar_driver import rplidar_driver
from tools.data_proc_func import *
from tools.lidar_cmd import empty_code
import re


class rplidar:
    def __init__(self,buflen,driver,painter):
        self.buflen = buflen
        self.driver = driver
        self.painter = painter
        self.status_ok = False
        self.last_start_angle = -1
        self.last_src_data = ''
        self.round_data = []
        self.last_result = []

    def startScan(self):
        '''
            开启扫描测距
        '''
        self.driver.sendCommand('GETINFO',self.driver.getInfo)
        self.driver.sendCommand('GETHELH',self.driver.getHealth)
        self.driver.sendCommand('EXPSCAN',self.driver.Scan)
        self.status_ok = True if self.driver.getStatus() == 'Work' and self.driver.healthOK() else False

    def statusOk(self):
        '''
            返回当前状态
        '''
        return self.status_ok

    def stop(self):
        '''
            停止扫描测距
        '''
        self.driver.sendCommand('STOP',self.driver.stop)

    def scanDataProcess(self,src_data):
        '''
            处理扫描数据
            - params
                - src_data:整包
        '''
        if len(src_data) == 2*self.buflen:
            start_angle =calcStartAngle(src_data)
            if self.last_src_data!='':
                cabin_list = re.findall(r'\w{10}',self.last_src_data[8:])
                result = parseCabinList(cabin_list,self.last_start_angle,start_angle)
                angle_diff = angleDiff(self.last_start_angle,start_angle)
                self.last_src_data = src_data
                self.last_start_angle = start_angle
                return result,angle_diff
            else:
                self.last_src_data = src_data
                self.last_start_angle = start_angle
                return empty_code,0
        else:
            return empty_code,0

    def gerneralProcess(self):
        '''
            数据处理核心
        '''
        src_data = self.driver.getResponse(buflen=self.buflen)
        result,angle_diff = self.scanDataProcess(src_data)
        if result != empty_code and self.last_result !=[]:
            index,include_header = getHeader(self.last_result,result,angle_diff)
            if include_header == True:
                round_data = self.round_data + result[index:]
                self.round_data = result[:index]
                self.last_result = result
                return sorted(round_data,key=lambda elem:elem[0])
            else:
                self.round_data += result
        self.last_result = result

    def drawLidarFrame(self,data):
        '''
            数据可视化
        '''
        return self.painter.drawLidarFrame(data)

if __name__ == '__main__':
    lidar = rplidar(buflen=84,driver=rplidar_driver(),painter=lidar_painter(1.5,400))
    lidar.startScan()
    while lidar.statusOk():
        round_data = lidar.gerneralProcess()
        if round_data:
            print(round_data)
            if lidar.drawLidarFrame(round_data):
                break
    lidar.stop()
    